/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2023-02-28     86188       the first version
 */
#include "fly_main.h"
#include "head_user.h"
#include "head_rtthread.h"
#include "math.h"

Task_HandleTypedef Task_Handle;

#define get_sign(x) ((x>0)? 1 : -1)
#define get_max(x, y) ((x>y)? x : y)
#define get_min(x, y) ((x>y)? y : x)

#define WPLimit 10

static void centre_controller_analyze(void);
static void Task_Stabilize(IMU_st *imu, Axis3f *gyro);
static void Task_Altitude(void);
static void capture_postion(char p);



struct rt_event task_event;
void task_stream(void* parameter)
{
    static uint8_t task_going_number = 10;

    rt_err_t result;
    result = rt_event_init(&task_event, "task_event", RT_IPC_FLAG_FIFO);
    if (result == RT_EOK)
    {
        rt_kprintf("task_event init finish.\n");
    }
    Task_Handle.CtrlSource.RC.priority   = 10;
    Task_Handle.CtrlSource.Fence.priority= 0xff;

    Task_Handle.CtrlSource.AUTO.priority = 8;
    Task_Handle.CtrlSource.AUTO.enable = 1;

    AUTO_Order* AUTO = &Task_Handle.CtrlSource.AUTO;

//    for (uint8_t i = 0; i < 12; i++)
//    {
//        AUTO->WayPoint.bytes[i] = Parame_PID.byte[85-11+i]; //////////////////////////////////航点
//    }

    while(1)
    {
        if( RC_Handle.Flyflag.Auto )
        {
            switch (task_going_number)
            {
                case 0://*起飞
                {
                    capture_postion(4);/*i 刷新定位点 i*/
                    AUTO->Valid.z = ENABLE;
                    AUTO->E_pos.z = 100;
                    AUTO->z_rate  = 50;

                    if (rt_event_recv(&task_event, TASK_EVENT_z_finish,
                                      RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR,
                                      RT_WAITING_FOREVER, RT_NULL) == RT_EOK)
                    {

                        rt_thread_mdelay(5000);
                        task_going_number = 2;
                    }
                }break;
                case 1://*降落
                {
                    AUTO->Valid.z = ENABLE;
                    AUTO->E_pos.z = 4;
                    AUTO->z_rate  = 30;

                    if (rt_event_recv(&task_event, TASK_EVENT_z_finish,
                                      RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR,
                                      RT_WAITING_FOREVER, RT_NULL) == RT_EOK)
                    {
                        rt_thread_mdelay(3000);
                        task_going_number = 99;
                    }
                }break;
                case 2://Y移动
                {
                    AUTO->Valid.y = ENABLE;
                    AUTO->E_pos.y = 250;
                    AUTO->xy_rate = 100;

                    if (rt_event_recv(&task_event, TASK_EVENT_y_finish,
                                      RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR,
                                      RT_WAITING_FOREVER, RT_NULL) == RT_EOK)
                    {

                        task_going_number = 3;
                    }
                }break;
                case 3://X移动
                {
                    AUTO->Valid.x = ENABLE;
                    AUTO->E_pos.x = 250;
                    AUTO->xy_rate = 100;

                    if (rt_event_recv(&task_event, TASK_EVENT_x_finish,
                                      RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR,
                                      RT_WAITING_FOREVER, RT_NULL) == RT_EOK)
                    {

                        task_going_number = 4;
                    }
                }break;
                case 4:
                {
                    AUTO->Valid.y = ENABLE;
                    AUTO->E_pos.y = 50;
                    AUTO->xy_rate = 100;

                    if (rt_event_recv(&task_event, TASK_EVENT_y_finish,
                                      RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR,
                                      RT_WAITING_FOREVER, RT_NULL) == RT_EOK)
                    {

                        task_going_number = 5;
                    }
                }break;
                case 5:
                {
                    AUTO->Valid.x = ENABLE;
                    AUTO->E_pos.x = 50;
                    AUTO->xy_rate = 100;

                    if (rt_event_recv(&task_event, TASK_EVENT_x_finish,
                                      RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR,
                                      RT_WAITING_FOREVER, RT_NULL) == RT_EOK)
                    {

                        task_going_number = 2;
                    }
                }break;
                case 10:
                {
                    AUTO->Valid.x = ENABLE;
                    AUTO->Valid.y = ENABLE;
                    AUTO->Valid.z = ENABLE;

                    AUTO->E_pos.x = AUTO->WayPoint.point[0].x;
                    AUTO->E_pos.y = AUTO->WayPoint.point[0].y;
                    AUTO->E_pos.z = AUTO->WayPoint.point[0].z;

                    AUTO->xy_rate = 100;
                    AUTO->z_rate  = 50;

                    if (rt_event_recv(&task_event, TASK_EVENT_x_finish|TASK_EVENT_y_finish|TASK_EVENT_z_finish,
                            RT_EVENT_FLAG_AND | RT_EVENT_FLAG_CLEAR,
                                      RT_WAITING_FOREVER, RT_NULL) == RT_EOK)
                    {

                        task_going_number = 11;
                    }
                }break;
                case 11:
                {
                    AUTO->Valid.x = ENABLE;
                    AUTO->Valid.y = ENABLE;
                    AUTO->Valid.z = ENABLE;

                    AUTO->E_pos.x = AUTO->WayPoint.point[1].x;
                    AUTO->E_pos.y = AUTO->WayPoint.point[1].y;
                    AUTO->E_pos.z = AUTO->WayPoint.point[1].z;

                    AUTO->xy_rate = 100;
                    AUTO->z_rate  = 50;

                    if (rt_event_recv(&task_event, TASK_EVENT_x_finish|TASK_EVENT_y_finish|TASK_EVENT_z_finish,
                            RT_EVENT_FLAG_AND | RT_EVENT_FLAG_CLEAR,
                                      RT_WAITING_FOREVER, RT_NULL) == RT_EOK)
                    {

                        task_going_number = 12;
                    }
                }break;
                case 12:
                {
                    AUTO->Valid.x = ENABLE;
                    AUTO->Valid.y = ENABLE;
                    AUTO->Valid.z = ENABLE;

                    AUTO->E_pos.x = AUTO->WayPoint.point[2].x;
                    AUTO->E_pos.y = AUTO->WayPoint.point[2].y;
                    AUTO->E_pos.z = AUTO->WayPoint.point[2].z;

                    AUTO->xy_rate = 100;
                    AUTO->z_rate  = 50;

                    if (rt_event_recv(&task_event, TASK_EVENT_x_finish|TASK_EVENT_y_finish|TASK_EVENT_z_finish,
                            RT_EVENT_FLAG_AND | RT_EVENT_FLAG_CLEAR,
                                      RT_WAITING_FOREVER, RT_NULL) == RT_EOK)
                    {

                        task_going_number = 13;
                    }
                }break;
                case 13:
                {
                    AUTO->Valid.x = ENABLE;
                    AUTO->Valid.y = ENABLE;
                    AUTO->Valid.z = ENABLE;

                    AUTO->E_pos.x = AUTO->WayPoint.point[3].x;
                    AUTO->E_pos.y = AUTO->WayPoint.point[3].y;
                    AUTO->E_pos.z = AUTO->WayPoint.point[3].z;

                    AUTO->xy_rate = 100;
                    AUTO->z_rate  = 50;

                    if (rt_event_recv(&task_event, TASK_EVENT_x_finish|TASK_EVENT_y_finish|TASK_EVENT_z_finish,
                            RT_EVENT_FLAG_AND | RT_EVENT_FLAG_CLEAR,
                                      RT_WAITING_FOREVER, RT_NULL) == RT_EOK)
                    {

                        task_going_number = 10;
                    }
                }break;
                case 99:
                {
                    RC_Handle.Flyflag.BodyState = lock;
                    task_going_number = 100;
                }break;
                case 100:
                {
                    rt_thread_mdelay(10);
                }break;
                default:break;
            }
        }
        else
        {
            //task_going_number = 0;
        }
        rt_thread_mdelay(10);   //100Hz
    }
}

void ctrl_stream(void* parameter)
{
    PID_Handle.All_PidInit();
    /* read fencepoint */
    for (uint8_t i = 0; i < 13; i++)
    {
        Task_Handle.CtrlSource.Fence.Fpoint.bytes[i] = Parame_PID.byte[70-11+i];
    }

    AUTO_Order* AUTO = &Task_Handle.CtrlSource.AUTO;
    for (uint8_t i = 0; i < 12; i++)
    {
        AUTO->WayPoint.bytes[i] = Parame_PID.byte[85-11+i]; //////////////////////////////////航点
    }

    static uint32_t tick;
    PID_Struct *pid = PID_Handle.pid;
    /* pid supplement_init */
    pid[Rol_Angle].dt = 1.0/500;
    pid[Pit_Angle].dt = 1.0/500;
    pid[Yaw_Angle].dt = 1.0/500;

    pid[Rol_Rate].dt = 1.0/500;
    pid[Pit_Rate].dt = 1.0/500;
    pid[Yaw_Rate].dt = 1.0/500;

    pid[Z_Position].dt  = 1.0/250;
    pid[Z_Speed].dt     = 1.0/250;
    pid[X_Position].dt  = 1.0/100;
    pid[Y_Position].dt  = 1.0/100;
    pid[X_Speed].dt     = 1.0/100;
    pid[Y_Speed].dt     = 1.0/100;
    while(1)
    {
        if(tick%5 == 0) //100HZ
        {
            centre_controller_analyze();
        }
        if(tick%2 == 0) //250HZ
        {
            Task_Altitude();
        }
        Task_Stabilize(BODY.angle, BODY.gyro);


        if( RC_Handle.Flyflag.Auto ){
            Task_Handle.CtrlSource.AUTO.enable = 1;
        }
        else {
            Task_Handle.CtrlSource.AUTO.enable = 0;
        }
        if( RC_Handle.Flyflag.Fence ){
            Task_Handle.CtrlSource.Fence.enable = 1;
        }
        else {
            Task_Handle.CtrlSource.Fence.enable = 0;
        }


        tick++;
        rt_thread_mdelay(2);    //500Hz
    }
}


static void capture_postion(char p)
{
    switch (p)
    {
        case 'x':
        {
            Task_Handle.staticPos.x = BODY.nfPos.x;
        }break;
        case 'y':
        {
            Task_Handle.staticPos.y = BODY.nfPos.y;
        }break;
        case 'z':
        {
            Task_Handle.staticPos.z = BODY.nfPos.z;
        }break;
        case 'a':
        {
            Task_Handle.staticPos.yaw = BODY.angle->yaw;
        }break;
        case 4:
        {
            Task_Handle.staticPos.x = BODY.nfPos.x;
            Task_Handle.staticPos.y = BODY.nfPos.y;
            Task_Handle.staticPos.z = BODY.nfPos.z;
            Task_Handle.staticPos.yaw = BODY.angle->yaw;
        }break;
        default:break;
    }
}
static void capture_postion2(char p, float pos)
{
    switch (p)
    {
        case 'x':
        {
            Task_Handle.staticPos.x = pos;
        }break;
        case 'y':
        {
            Task_Handle.staticPos.y = pos;
        }break;
        case 'z':
        {
            Task_Handle.staticPos.z = pos;
        }break;
        case 'a':
        {
            Task_Handle.staticPos.yaw = pos;
        }break;
        default:break;
    }
}

static inline void CtrlReset(void)
{
    Task_Handle.Ctrl.priority.x = 0;
    Task_Handle.Ctrl.priority.y = 0;
    Task_Handle.Ctrl.priority.z = 0;
    Task_Handle.Ctrl.priority.yaw = 0;

    Task_Handle.Ctrl.speed.x = 0;
    Task_Handle.Ctrl.speed.y = 0;
    Task_Handle.Ctrl.speed.z = 0;
    Task_Handle.Ctrl.speed.yaw = 0;
}

void centre_controller_analyze(void)
{
    static uint8_t lock_lock;
    double theta;
    float z_error, yaw_error;

    PID_Struct *pid = PID_Handle.pid;
    Ctrl_Package *ctrl = &Task_Handle.Ctrl;
    vector *nowPos = &Task_Handle.nowPos;
    vector *staticPos = &Task_Handle.staticPos;

    /* 进入自动任务，解锁一次飞控 */
    if (RC_Handle.Flyflag.BodyState == unlock && lock_lock == 0)
    {
        lock_lock = 1;
        /* record take-off point */
        Task_Handle.zeroPoint.x = BODY.nfPos.x;
        Task_Handle.zeroPoint.y = BODY.nfPos.y;

        capture_postion(4);
    }

    if (RC_Handle.Flyflag.BodyState == unlock)
    {
        /* update current position */
        nowPos->x = BODY.nfPos.x;
        nowPos->y = BODY.nfPos.y;
        nowPos->z = BODY.nfPos.z;
        nowPos->yaw = BODY.angle->yaw;

        /* fence contral */
        /*===============================================================================================*/
        if (Task_Handle.CtrlSource.Fence.enable)
        {
            Fence_contral *current = &Task_Handle.CtrlSource.Fence;
            float F12, F23, F34, F41;
            F12 = (current->Fpoint.fencepoint[1].x-current->Fpoint.fencepoint[0].x)*(BODY.nfPos.y-current->Fpoint.fencepoint[0].y)- \
                  (current->Fpoint.fencepoint[1].y-current->Fpoint.fencepoint[0].y)*(BODY.nfPos.x-current->Fpoint.fencepoint[0].x);
            F23 = (current->Fpoint.fencepoint[2].x-current->Fpoint.fencepoint[1].x)*(BODY.nfPos.y-current->Fpoint.fencepoint[1].y)- \
                  (current->Fpoint.fencepoint[2].y-current->Fpoint.fencepoint[1].y)*(BODY.nfPos.x-current->Fpoint.fencepoint[1].x);
            F34 = (current->Fpoint.fencepoint[3].x-current->Fpoint.fencepoint[2].x)*(BODY.nfPos.y-current->Fpoint.fencepoint[2].y)- \
                  (current->Fpoint.fencepoint[3].y-current->Fpoint.fencepoint[2].y)*(BODY.nfPos.x-current->Fpoint.fencepoint[2].x);
            F41 = (current->Fpoint.fencepoint[0].x-current->Fpoint.fencepoint[3].x)*(BODY.nfPos.y-current->Fpoint.fencepoint[3].y)- \
                  (current->Fpoint.fencepoint[0].y-current->Fpoint.fencepoint[3].y)*(BODY.nfPos.x-current->Fpoint.fencepoint[3].x);

//            Debug_Handle.val[0] = F12/1000.0f;
//            Debug_Handle.val[1] = F23/1000.0f;
//            Debug_Handle.val[2] = F34/1000.0f;
//            Debug_Handle.val[3] = F41/1000.0f;
            /* outside the fence */
            if ( (BODY.nfPos.z > get_max(current->Fpoint.fencepointHZ, current->Fpoint.fencepointLZ) || BODY.nfPos.z < get_min(current->Fpoint.fencepointHZ, current->Fpoint.fencepointLZ))|| \
                 (!((F12>0 && F23>0 && F34>0 && F41>0) || (F12<0 && F23<0 && F34<0 && F41<0))) )
            {
                capture_postion2('x', current->Fpoint.home.x);//home
                capture_postion2('y', current->Fpoint.home.y);//home
                capture_postion2('z', current->Fpoint.home.z);//home

                current->Valid.yaw = 1;
            }
            if( current->Valid.yaw )
            {
                ctrl->priority.x = current->priority;
                ctrl->priority.y = current->priority;
                ctrl->priority.z = current->priority;

                /* came home */
                if( (F12>0 && F23>0 && F34>0 && F41>0) || (F12<0 && F23<0 && F34<0 && F41<0) )
                {
                    current->Valid.yaw = 0;
                }
            }
       }

        /*===============================================================================================*/
        /* remote contral */
        if (1)
        {
            RC_contral *contral = &Task_Handle.CtrlSource.RC;
            if ( (RC_Handle.rc_dat[THR] != 0) && (contral->priority > ctrl->priority.z) )
            {
                ctrl->speed.z = RC_Handle.rc_dat[THR] * 0.4;
                ctrl->priority.z = contral->priority;
                contral->lock.z = 0;
            }
            if ( (RC_Handle.rc_dat[ROL] != 0) && (contral->priority > ctrl->priority.x) )
            {
                ctrl->speed.x = RC_Handle.rc_dat[ROL] * 0.4;
                ctrl->priority.x = contral->priority;
                contral->lock.x = 0;
            }
            if ( (RC_Handle.rc_dat[PIT] != 0) && (contral->priority > ctrl->priority.y) )
            {
                ctrl->speed.y = -RC_Handle.rc_dat[PIT] * 0.4;
                ctrl->priority.y = contral->priority;
                contral->lock.y = 0;
            }
            if ( (RC_Handle.rc_dat[YAW] != 0) && (contral->priority > ctrl->priority.yaw) )
            {
                ctrl->speed.yaw = RC_Handle.rc_dat[YAW] * 0.25;
                ctrl->priority.yaw = contral->priority;
                contral->lock.yaw = 0;
            }
            if(RC_Handle.rc_dat[THR] == 0 && contral->lock.z == 0)
            {
                contral->lock.z = 1;
                capture_postion('z');
            }
            if(RC_Handle.rc_dat[ROL] == 0 && contral->lock.x == 0)
            {
                contral->lock.x = 1;
                capture_postion('x');
            }
            if(RC_Handle.rc_dat[PIT] == 0 && contral->lock.y == 0)
            {
                contral->lock.y = 1;
                capture_postion('y');
            }
//            if(RC_Handle.rc_dat[YAW] == 0 && contral->lock.yaw == 0)
//            {
//                contral->lock.yaw = 1;
//                capture_postion('a');
//            }
            if(RC_Handle.rc_dat[YAW] == 0 && contral->lock.yaw == 0 && fabsf(BODY.gyro->z)<20)
            {
                contral->lock.yaw = 1;
                capture_postion('a');
            }
            else if(RC_Handle.rc_dat[YAW] == 0 && contral->lock.yaw == 0)
            {
                ctrl->speed.yaw = 0;
                ctrl->priority.yaw = contral->priority;
            }
        }
        /*===============================================================================================*/

        /*===============================================================================================*/
        /*-------------------------------------------------------------------*
         * 程序指令控制部分（基于世界坐标系，在飞机刚启动时构建）
         *-------------------------------------------------------------------*/
        if (Task_Handle.CtrlSource.AUTO.enable)
        {
            AUTO_Order *current = &Task_Handle.CtrlSource.AUTO;
            Axis2f nf = {0}, bf = {0};

            /* x方向移动  */
            if ((current->priority > ctrl->priority.x) && current->Valid.x)
            {
                /* calculate error under body frame */
                nf.x += current->E_pos.x - nowPos->x;
                nf.y += 0;

                if (fabsf(nf.x) < WPLimit)
                {
                    current->Valid.x = 0;
                    capture_postion2('x', current->E_pos.x);
                    rt_event_send(&task_event, TASK_EVENT_x_finish);

                    ctrl->finish.x = finished;
                }
                else
                {
                    ctrl->finish.x = performing;
                    ctrl->priority.x = current->priority;
                }
            }
            /* y方向移动  */
            if ((current->priority > ctrl->priority.y) && current->Valid.y)
            {
                /* calculate error under body frame */
                nf.x += 0;
                nf.y += current->E_pos.y - nowPos->y;

                if (fabsf(nf.y) < WPLimit)
                {
                    current->Valid.y = 0;
                    capture_postion2('y', current->E_pos.y);
                    rt_event_send(&task_event, TASK_EVENT_y_finish);

                    ctrl->finish.y = finished;
                }
                else
                {
                    ctrl->finish.y = performing;
                    ctrl->priority.y = current->priority;
                }
            }
            if( ctrl->priority.y == current->priority || ctrl->priority.x == current->priority )
            {
                BODY.Navi_To_Body_2(&bf, &nf, -BODY.angle->yaw);
                theta = atan2(bf.x, bf.y);          //x/y,对应的正切角(0~2pi全映射)

                ctrl->speed.x += sin(theta) * current->xy_rate;
                ctrl->speed.y += cos(theta) * current->xy_rate;
            }

            /* Z方向控制 */
            if ((current->priority > ctrl->priority.z) && current->Valid.z)
            {
                z_error = current->E_pos.z - nowPos->z;

                Debug_Handle.val[0] = current->E_pos.z;
                if (fabsf(z_error) < 10)
                {
                    ctrl->speed.z = PID_Handle.error_Updata(&pid[Z_Position], z_error);

                    current->Valid.z = 0;
                    capture_postion2('z', current->E_pos.z);
                    rt_event_send(&task_event, TASK_EVENT_z_finish);

                    ctrl->finish.z = finished;
                }
                else
                {
                    ctrl->speed.z = get_sign(z_error) * current->z_rate;

                    ctrl->finish.z = performing;
                }
                ctrl->priority.z = current->priority;
            }
            /*i 自旋控制 i*/
            if (current->priority > ctrl->priority.yaw && current->Valid.yaw)
            {
                yaw_error = current->E_pos.yaw - nowPos->yaw;
                if (yaw_error > +180)
                    yaw_error -= 360.0f;
                if (yaw_error < -180)
                    yaw_error += 360.0f;

                if (fabsf(yaw_error) < 10)
                {
                    ctrl->speed.yaw = PID_Handle.error_Updata(&pid[YAW], yaw_error);

                    current->Valid.yaw = 0;
                    capture_postion2('a', current->E_pos.yaw);
                    rt_event_send(&task_event, TASK_EVENT_yaw_finish);

                    ctrl->finish.yaw = finished;
                }
                else
                {
                    ctrl->speed.yaw = PID_Handle.error_Updata(&pid[YAW], yaw_error);

                    ctrl->finish.yaw = performing;
                }
                ctrl->priority.yaw = current->priority;
            }
        }

        /* 无控制源时默认定点悬停,保底控制, 最高优先级下进行围栏控制 */
        Axis2f nf = {0}, bf = {0};
        if(ctrl->priority.x == 0 || ctrl->priority.x == 0xff)
        {
            pid[X_Position].Expect = staticPos->x;

            /* calculate the error of body frame */
            nf.x = pid[X_Position].Expect - BODY.nfPos.x;
            nf.y = 0;
            BODY.Navi_To_Body_2(&bf, &nf, -BODY.angle->yaw);

            ctrl->speed.x += PID_Handle.error_Updata(&pid[X_Position], bf.x);
            ctrl->speed.y += PID_Handle.error_Updata(&pid[Y_Position], bf.y);
        }
        if(ctrl->priority.y == 0 || ctrl->priority.y == 0xff)
        {
            pid[Y_Position].Expect = staticPos->y;

            /* calculate the error of body frame */
            nf.x = 0;
            nf.y = pid[Y_Position].Expect - BODY.nfPos.y;
            BODY.Navi_To_Body_2(&bf, &nf, -BODY.angle->yaw);

            ctrl->speed.x += PID_Handle.error_Updata(&pid[X_Position], bf.x);
            ctrl->speed.y += PID_Handle.error_Updata(&pid[Y_Position], bf.y);
        }
        if (ctrl->priority.z == 0 || ctrl->priority.z == 0xff)
        {
            pid[Z_Position].Expect = staticPos->z;
            ctrl->speed.z = PID_Handle.Update(&pid[Z_Position], BODY.nfPos.z);
        }
        if (ctrl->priority.yaw == 0 || ctrl->priority.yaw == 0xff)
        {
            pid[Yaw_Angle].Expect = staticPos->yaw;
            ctrl->speed.yaw = PID_Handle.Yaw_angle_Updata(&pid[Yaw_Angle], BODY.angle->yaw);
        }
    }

    /* result output */
    if ( RC_Handle.Flyflag.Position )
    {
        pid[X_Speed].Expect = ctrl->speed.x;
        pid[Y_Speed].Expect = ctrl->speed.y;
        PID_Handle.Update(&pid[X_Speed], BODY.bfSpeed.x);
        PID_Handle.Update(&pid[Y_Speed], BODY.bfSpeed.y);

        pid[Rol_Angle].Expect = limit(pid[X_Speed].OutPut*( 0.06f), -30, 30);
        pid[Pit_Angle].Expect = limit(pid[Y_Speed].OutPut*(-0.06f), -30, 30);
    }
    else
    {
        pid[Rol_Angle].Expect = RC_Handle.datswitch(RC_Handle.rc_uint[ROL], 30);
        pid[Pit_Angle].Expect = RC_Handle.datswitch(RC_Handle.rc_uint[PIT], 30);
    }

    pid[Z_Speed].Expect  = ctrl->speed.z;
    pid[Yaw_Rate].Expect = ctrl->speed.yaw;

    Debug_Handle.val[0] = ctrl->priority.x;
    Debug_Handle.val[1] = ctrl->priority.y;
    Debug_Handle.val[2] = ctrl->priority.z;

    CtrlReset();
}

static void Task_Stabilize(IMU_st *imu, Axis3f *gyro)
{
    PID_Struct *pid = PID_Handle.pid;

    /* update waypoints-ctrl output */
    pid[Rol_Rate].Expect = PID_Handle.Update(&pid[Rol_Angle], 0);
    pid[Pit_Rate].Expect = PID_Handle.Update(&pid[Pit_Angle], 0);
    PID_Handle.Update(&pid[Yaw_Rate], gyro->z);

    PID_Handle.Update(&pid[Rol_Rate], 0);
    PID_Handle.Update(&pid[Pit_Rate], 0);

    pid[stableRol].Expect = 0;
    pid[stablePit].Expect = 0;
    PID_Handle.angle_Update(&pid[stableRol], imu->roll,  gyro->x);
    PID_Handle.angle_Update(&pid[stablePit], imu->pitch, gyro->y);
}
static void Task_Altitude(void)
{
    PID_Struct *pid = PID_Handle.pid;

    /* update waypoints-ctrl output */
    PID_Handle.Update(&pid[Z_Speed], BODY.nfSpeed.z);
}


